
#include "tmRosNodeHandle.h"

TmRosNodeHandle::TmRosNodeHandle()
{
    qRegisterMetaType<S_AngleRosPubType>("S_AngleRosPubType");
    // tf::TransformBroadcaster broadcaster;
    // ros::Rate loop_rate(30);

    // const double degree = M_PI / 180;

    // robot state
    //     double inc= 0.005, base_arm_inc= 0.005, arm1_armbase_inc= 0.005, arm2_arm1_inc= 0.005, gripper_inc= 0.005, tip_inc= 0.005;
    // double angle = 0, base_arm = 0, arm1_armbase = 0, arm2_arm1 = 0, gripper = 0, tip = 0;
    // double angleArray[6] = {0.5, 0.5, 0.5, -0.5, -0.5, -0.5};
    // uint32_t turnNum = 0;

    moveToThread(this);
    start();
}

void TmRosNodeHandle::run()
{
    ros::VP_string remapping_args; 
    const std::string name;
    int argc=0;
    char **argv;
    // char argv1[20]={"tmarm_tinnu"};
    ros::init(argc, (char **)argv, "tmarm_fb");
    mpRosNodeHandle = new ros::NodeHandle;
    mpJointPub = new ros::Publisher;
    mpJointState = new sensor_msgs::JointState;
    // ros::init(remapping_args, name, "tmarm_fb",);

    *mpJointPub = mpRosNodeHandle->advertise<sensor_msgs::JointState>("joint_states", 1);
    connect(this, &TmRosNodeHandle::SnNodePublic, this,
            [this](char which, S_AngleRosPubType data)
            {
                if (ros::ok())
                {
                    // qDebug() << "Sn node" << data.r;
                    mpJointState->header.stamp = ros::Time::now();
                    mpJointState->name.resize(3);
                    mpJointState->position.resize(3);
                    mpJointState->name[0] = "pointp";
                    mpJointState->position[0] = data.p;
                    mpJointState->name[1] = "pointv";
                    mpJointState->position[1] = data.v;
                    mpJointState->name[2] = "pointr";
                    mpJointState->position[2] = data.r;

                    mpJointPub->publish(*mpJointState);
                }
                else
                {
                    qDebug() << "end";
                    quit();
                }
            });
    exec();
}